/*
 * TestForm.java
 *
 * Created on 13 de noviembre de 2007, 04:13 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;

import Behaviors.Arbitrators.*;
import Behaviors.Formations.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Behaviors.Schemas.*;
import Controllers.*;
import Robot.*;
import Robot.Sensors.Sonar;
import Util.*;


/**
 *
 * @author nxbot
 */
public class TestForm2 {
    
    /**
     * Creates a new instance of TestForm
     */
    public static void process(){
    
        //SimulationInterface simu=null;
        
        VirtualRobot robotLeader = new VirtualRobot("Virtual",5);
        VirtualPosition2d positionLeader = new VirtualPosition2d(robotLeader,"p",5);
        

        //***Definicion de Robots ****************************************************
        //Robot robotLeader=new Robot("127.1.1.1", 6665, "Leader",0);
        Robot robot1=new Robot("127.1.1.1",6666,"Robot 1",1);
        Robot robot2=new Robot("127.1.1.1",6667,"Robot 2",2);
        Robot robot3=new Robot("127.1.1.1",6668,"Robot 4",4);
        Robot robot4=new Robot("127.1.1.1",6669,"Robot 3",3);
        
        //
        //Robot obstConnection1=new Robot("127.1.1.1",6670,"obstacles1",5);
        //Robot obstConnection2=new Robot("127.1.1.1",6671,"obstacles2",6);
        
        //simu=robotLeader.getConnection().requestInterfaceSimulation(0,PlayerConstants.PLAYER_OPEN_MODE);
        
        
        //RealPosition2d positionLeader=new RealPosition2d(robotLeader,"positionCenter",0);
        RealPosition2d position1=new RealPosition2d(robot1,"position1",0);
        RealPosition2d position2=new RealPosition2d(robot2,"position2",0);
        RealPosition2d position3=new RealPosition2d(robot3,"position3",0);
        RealPosition2d position4=new RealPosition2d(robot4,"position4",0);
        
        //Sonar sonarLeader = new Sonar(robotLeader,"sonarCenter");
//        Sonar sonar1 = new Sonar(robot1,"sonar1",0);
//        Sonar sonar2 = new Sonar(robot2,"sonar2",0);
//        Sonar sonar3 = new Sonar(robot3,"sonar3",0);
//        Sonar sonar4 = new Sonar(robot4,"sonar4",0);
        
        //obstacles
        //RealPosition2d obst1 = new RealPosition2d(obstConnection1,"positionObst1",0);
        //RealPosition2d obst2 = new RealPosition2d(obstConnection2,"positionObst2",0);
        

        //****************************************************************************

       // robotLeader.runThreaded(10,0);
        robot1.runThreaded(10,0);
        robot2.runThreaded(10,0);
        robot3.runThreaded(10,0);
        robot4.runThreaded(10,0);
        
        //obstConnection1.runThreaded(10,0);
        //obstConnection2.runThreaded(10,0);
        
        
        //position4.setInitialPosition(new Pose2d(-2.0f,-13.0f,(float)Math.toRadians(90.0f)));
        //pos.setInitialPosition(new Pose2d(0.0f,-10.0f,(float)Math.toRadians(90.0f)));
        
        //pos.setSpeed(0.3f,0.2f);
        //position4.setSpeed(1.0f,-0.5f);
        
        //while(!sonarLeader.updatePoses());

//        while(!sonar1.updatePoses());
//        while(!sonar2.updatePoses());
//        while(!sonar3.updatePoses());
//        while(!sonar4.updatePoses());
//        
        //Formations to be used
        FormationSquare formQuad = new FormationSquare(4);
        FormationArrow formArrow = new FormationArrow(4);
        FormationRow formRow = new FormationRow (4);
        FormationColumn formCol = new FormationColumn (4);
        
        Formation[] array = {formQuad, formArrow, formRow, formCol};
        FormationGenerator formation=new FormationGenerator(array);

       // JFrameChangeForm frame = new JFrameChangeForm(formation);

        //positionLeader.setInitialPosition(new Pose2d(0,-10,(float)Math.toRadians(90.0f)));
        /*
         *
         **/
        positionLeader.setInitialPosition(new Pose2d(0.0f,0.0f,(float)Math.toRadians(90.0f)));
        position1.setInitialPosition(new Pose2d(-1.0f,0.0f,(float)Math.toRadians(90.0f)));
        position2.setInitialPosition(new Pose2d(1.0f,0.0f,(float)Math.toRadians(90.0f)));
//        positionLeader.setInitialPosition(new Pose2d(0,-10,(float)Math.toRadians(0.0f)));
//        position1.setInitialPosition(new Pose2d(-2.0f,-13.0f,(float)Math.toRadians(90.0f)));
//        position2.setInitialPosition(new Pose2d(2.0f,-13.0f,(float)Math.toRadians(90.0f)));
        position3.setInitialPosition(new Pose2d(-2.0f,-15.0f,0.0f));
        position4.setInitialPosition(new Pose2d(2.0f,-15.0f,0.0f));
        
        //obst1.setInitialPosition(new Pose2d(1.0f,0.0f,0.0f));
        //obst2.setInitialPosition(new Pose2d(-1.0f,0.0f,0.0f));
        
        Vector2d point=new Vector2d(0,15,Vector2d.MODE_XY);

        System.out.println("PointAngleA="+Float.toString(point.getAngle()));


        //PSDetectLeader detectLeader = new PSDetectLeader(positionLeader);
        //MSFollowLeader followLeader = new MSFollowLeader(positionFollower, detectLeader);


        MSGoToPoint goto2 = new MSGoToPoint("goto2",positionLeader,point,MSGoToPoint.MODE_LINEAR);
        
        //MSGoToPoint goto2 = new MSGoToPoint(position1,point,MSGoToPoint.MODE_LINEAR);
        
        //goto2.setMaxVel(0.2f);

        PSDetectFormation detectFormation1=new PSDetectFormation("goto2",formation,position1,positionLeader);
        PSDetectFormation detectFormation2=new PSDetectFormation("goto2",formation,position2,positionLeader);
        PSDetectFormation detectFormation3=new PSDetectFormation("goto2",formation,position3,positionLeader);
        PSDetectFormation detectFormation4=new PSDetectFormation("goto2",formation,position4,positionLeader);

        MSGoToPoint maintain1 = new MSGoToPoint("goto2",detectFormation1,position1,MSGoToPoint.MODE_EXPONENTIAL);
        MSGoToPoint maintain2 = new MSGoToPoint("goto2",detectFormation2,position2,MSGoToPoint.MODE_EXPONENTIAL);
        MSGoToPoint maintain3 = new MSGoToPoint("goto2",detectFormation3,position3,MSGoToPoint.MODE_EXPONENTIAL);
        MSGoToPoint maintain4 = new MSGoToPoint("goto2",detectFormation4,position4,MSGoToPoint.MODE_EXPONENTIAL);
        
        maintain1.setExponentialDistance(30.0f);
        maintain2.setExponentialDistance(30.0f);
        maintain3.setExponentialDistance(30.0f);
        maintain4.setExponentialDistance(30.0f);


//maintain1.setMaxVel(0.6f);
        //maintain2.setMaxVel(0.6f);
        //maintain3.setMaxVel(0.6f);
        //maintain4.setMaxVel(0.6f);
        
        maintain1.setExponentialDistance(1);
        maintain2.setExponentialDistance(1);
        maintain3.setExponentialDistance(1);
        maintain4.setExponentialDistance(1);
        

        Vector2d[] obst = {new Vector2d(-1.3f,0,Vector2d.MODE_XY),new Vector2d(1.3f,0,Vector2d.MODE_XY)};
        //Position2d[] obstP = {obst1,obst2};
        //PSDetectObstaclesVirtualObj obstaclesVirt = new PSDetectObstaclesVirtualObj (positionLeader,obstP,0.5f);
        //PSDetectObstaclesVirtualObj obstaclesVirt = new PSDetectObstaclesVirtualObj (position1,obstP,0.5f);
        
//        PSDetectObstaclesSonar obstacles1 = new PSDetectObstaclesSonar(position1,sonar1);
//        PSDetectObstaclesSonar obstacles2 = new PSDetectObstaclesSonar(position2,sonar2);
//        PSDetectObstaclesSonar obstacles3 = new PSDetectObstaclesSonar(position3,sonar3);
//        PSDetectObstaclesSonar obstacles4 = new PSDetectObstaclesSonar(position4,sonar4);

//        MSAvoidObstacles aobst1 = new MSAvoidObstacles(obstacles1,position1); 
//        MSAvoidObstacles aobst2 = new MSAvoidObstacles(obstacles2,position2); 
//        MSAvoidObstacles aobst3 = new MSAvoidObstacles(obstacles3,position3); 
//        MSAvoidObstacles aobst4 = new MSAvoidObstacles(obstacles4,position4); 
        //MSAvoidObstacles aobstVirtual = new MSAvoidObstacles(obstaclesVirt,positionLeader); 
        
        //MSAvoidObstacles aobst1 = new MSAvoidObstacles(obstaclesVirt,position1); 
         
        //PSChangeFormation chform1 = new PSChangeFormation (positionLeader, formation, obstaclesVirt, detectFormation1);
        
        Schema[] arrayLeader = {goto2};//,chform1, aobstVirtual};//{chform1};//,aobst1};//  {maintain1, aobst1};
        
        Schema[] array1 = {maintain1};//, aobst1};//{chform1};//,aobst1};//  {maintain1, aobst1};
        Schema[] array2 = {maintain2};//, aobst2};// {chform2};//, aobst2};
        Schema[] array3 =  {maintain3};//, aobst3};//{chform3};//, aobst3};
        Schema[] array4 =  {maintain4};//, aobst4};//{chform4};//, aobst4};
        
        //Schema[] array1 = {goto2, aobst1};//{chform1};//,aobst1};//  {maintain1, aobst1};
        
        /*
        Schema[] array1 = {chform1};//,aobst1};//  {maintain1, aobst1};
        Schema[] array2 = {chform2};//, aobst2};
        Schema[] array3 = {chform3};//, aobst3};
        Schema[] array4 = {chform4};//, aobst4};
        */
        //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
        VectorAdditionArbitrator arbitratorLeader=new VectorAdditionArbitrator(arrayLeader,positionLeader, false);
        VectorAdditionArbitrator arbitrator1=new VectorAdditionArbitrator(array1,position1, false);
        VectorAdditionArbitrator arbitrator2=new VectorAdditionArbitrator(array2,position2, false);
        VectorAdditionArbitrator arbitrator3=new VectorAdditionArbitrator(array3,position3, false);
        VectorAdditionArbitrator arbitrator4=new VectorAdditionArbitrator(array4,position4, false);

        BehaviorBasedController controllerGoto=new BehaviorBasedController("goto",robotLeader,arbitratorLeader,10);
        BehaviorBasedController controller1=new BehaviorBasedController("Robot1 Controller",robot1,arbitrator1,10);
        BehaviorBasedController controller2=new BehaviorBasedController("Robot2 Controller",robot2,arbitrator2,10);
        BehaviorBasedController controller3=new BehaviorBasedController("Robot3 Controller",robot3,arbitrator3,10);
        BehaviorBasedController controller4=new BehaviorBasedController("Robot4 Controller",robot4,arbitrator4,10);
        
        Thread hiloLeader=new Thread(controllerGoto);
        Thread hilo1=new Thread(controller1);
        Thread hilo2=new Thread(controller2);
        Thread hilo3=new Thread(controller3);
        Thread hilo4=new Thread(controller4);

        //pos.setDebugMode(true);
        //frame.setVisible(true);

        hiloLeader.start();
        hilo1.start();
        hilo2.start();
        hilo3.start();
        hilo4.start();
        /*
        while(true){
            
            //System.out.print("X Pos of virtual Robot: ");
            //System.out.println(
            //simu.set2DPose("gort",pos.read(Sensor.READ_MODE_NO_WAIT));
            //simu.setPosition((byte)0x01);
            //float x1 = pos.read(Sensor.READ_MODE_NO_WAIT).getPx();
            //float y1 = pos.read(Sensor.READ_MODE_NO_WAIT).getPy();
            //float angle = pos.read(Sensor.READ_MODE_NO_WAIT).getPa();
            position4.read(Sensor.READ_MODE_NO_WAIT);
            //float y = position4.read(Sensor.READ_MODE_NO_WAIT).getPy();
            //float ax = position4.read(Sensor.READ_MODE_NO_WAIT).getPa();
            
            //System.out.println("Leader: Pos X "+Double.toString(x)+" Y: "+Double.toString(y));
            //System.out.println("Simulated: Pos X "+Double.toString(x1)+" Y: "+Double.toString(y1));
            //System.out.println("Simulated: Angle: "+Double.toString(angle)+" Real: "+Double.toString(ax));
            
            
            try{
                Thread.sleep(100);
            }
            catch(Exception e){}

        }*/
    }
}